#include <math.h>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include "geometry_msgs/Vector3.h"
#include "/home/dalton/ros_workspace/pi_tracker/msg_gen/cpp/include/pi_tracker/Skeleton.h"

float distance(float *a, float *b){
  
  return sqrt((a[0]-b[0])*(a[0]-b[0])+(a[1]-b[1])*(a[1]-b[1])+(a[2]-b[2])*(a[2]-b[2]));
}

float skeletonDistance(pi_tracker::Skeleton::ConstPtr msg, int a, int b){
  
  float u[3];
  float v[3];
  
  u[0]=(float) msg->position[a].x;
  u[1]=(float) msg->position[a].y;
  u[2]=(float) msg->position[a].z;
  
  v[0]=(float) msg->position[b].x;
  v[1]=(float) msg->position[b].y;
  v[2]=(float) msg->position[b].z;
  
  return distance(u,v);
  
}

void chatterCallback(const pi_tracker::Skeleton::ConstPtr &msg)
{
 
    
  std::cout << "right arm size: "<<skeletonDistance(msg,8,7)<<std::endl;
  std::cout << "left arm size: "<<skeletonDistance(msg,4,5)<<std::endl;
  std::cout << "right leg size: "<<skeletonDistance(msg,13,14)<<std::endl;
  std::cout << "left leg size: "<<skeletonDistance(msg,10,11)<<std::endl;
  std::cout << "distance between shoulders: "<<skeletonDistance(msg,3,6)<<std::endl;
  std::cout << "Height estimate: "<<(skeletonDistance(msg,0,11)+skeletonDistance(msg,0,14))/2 + skeletonDistance(msg,0,1)/2<<std::endl;
  std::cout << "arm span: "<<skeletonDistance(msg,3,4)+skeletonDistance(msg,4,5)+skeletonDistance(msg,3,6)+skeletonDistance(msg,6,7)+skeletonDistance(msg,7,8)<<std::endl;
  //ROS_INFO("I heard: [%f]", msg->position[0].x);
}

int main(int argc, char **argv)
{
  /**right
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "distances");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The subscribe() call is how you tell ROS that you want to receive messages
   * on a given topic.  This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing.  Messages are passed to a callback function, here
   * called chatterCallback.  subscribe() returns a Subscriber object that you
   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
   * object go out of scope, this callback will automatically be unsubscribed from
   * this topic.
   *
   * The second parameter to the subscribe() function is the size of the message
   * queue.  If messages are arriving faster than they are being processed, this
   * is the number of messages that will be buffered up before beginning to throw
   * away the oldest ones.
   */
  ros::Subscriber sub = n.subscribe("skeleton", 1000, chatterCallback);
  //ros::Subscriber sub = n.subscribe("/skeleton", 1000, ftest);

  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one).  ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */
  ros::spin();

  return 0;
} 
